#include <iostream>
#include <bits/stdc++.h>
#include "ag_robot/cooperate_game.h"
#include <Eigen/Core>                    // 矩阵运算
#include <Eigen/QR>


constexpr double pi() { return M_PI; }

namespace COOP
{
COOPFunction::COOPFunction(double _thhold_1,double _thhold_2)
{
    thhold_1 = _thhold_1;
    thhold_2 = _thhold_2;
    Repulsor_point = 0.5;
    Repulsor_point_max = 0.5;
    Virtual_Force_Real = 0.0;
    size_k = 0.1;
    k_h = 0.8;
    k_r = 0.2;
    RobotCommand.resize(4);
    HistoryCommand.resize(6);
    HistoryCommand << 0,0,0,0,0,0;
}

COOPFunction::COOPFunction()
{
    Repulsor_point = 0.5;
    Repulsor_point_max = 0.5;
    Virtual_Force_Real = 0.0;
    size_k = 0.1;
    k_h = 0.8;
    k_r = 0.2;
    RobotCommand.resize(4);
    HistoryCommand.resize(6);
    HistoryCommand << 0,0,0,0,0,0;
}

// Update VirtualForce Prodict
double COOPFunction::VirtualForcePredictUpdate(double _Virtual_Force_Real)
{
   double RP = Repulsor_point;
   double RPm = 0.5;
   double T3 = (1-RP)*8/3;
   double T4 = T3/2;
   double T5 = 2*RP/3*4;
   double T6 = (1-RP)*8/3;
   double T7 = T6/2;
   double VirtualDeltaForce;
    // T7 T6 T5 T1 T2 T3 T4
    // T1
    // T1
   if(abs(_Virtual_Force_Real) <= RP/3)
   {
    VirtualDeltaForce = -size_k*RP/RPm*sin(3*pi()/(RP*2)*_Virtual_Force_Real);
   }    
    // T2
   else
   if(_Virtual_Force_Real > RP/3 && _Virtual_Force_Real <= RP)
   {
    VirtualDeltaForce = size_k*RP/RPm*sin(3.0*pi()/(RP*4)*_Virtual_Force_Real-3.0*pi()/4);
   } 
    // T3
   else
   if(_Virtual_Force_Real > RP && _Virtual_Force_Real <= (2/3 + RP/3))
   {
    VirtualDeltaForce = size_k*(1-RP)/(1-RPm)*sin(2*pi()/T3*(_Virtual_Force_Real-RP));
   }   
    // T4
   else
   if(_Virtual_Force_Real > (2/3 + RP/3))
   {
    VirtualDeltaForce = -size_k*(1-RP)/(1-RPm)*sin(2*pi()/T4*(_Virtual_Force_Real-1));
   } 
    // T5
   else
   if(_Virtual_Force_Real < -RP/3 && _Virtual_Force_Real > -RP)
   {
    VirtualDeltaForce = size_k*RP/RPm*sin(2*pi()/T5*(_Virtual_Force_Real+RP));
   }
    // T6
   else
   if(_Virtual_Force_Real <= -RP && _Virtual_Force_Real > (-2/3-RP/3))
   {
    VirtualDeltaForce = size_k*(1-RP)/(1-RPm)*sin(2*pi()/T6*(_Virtual_Force_Real+RP));
   }    
    // T7
   else
   if(_Virtual_Force_Real <= (-2/3-RP/3))
   {
    VirtualDeltaForce = -size_k*(1-RP)/(1-RPm)*sin(2*pi()/T7*(_Virtual_Force_Real+1));
   }
    double output = _Virtual_Force_Real + VirtualDeltaForce;
    return output;
}

void COOPFunction::Get_MPC_Command()
{
    double Virtual_Force_Predict = Virtual_Force_Real;
    //mpc_steps: 4
    for(int i = 0 ;i <= 3 ; i++)
    {
        Virtual_Force_Predict = VirtualForcePredictUpdate(Virtual_Force_Predict);
        RobotCommand(i) = Virtual_Force_Predict;
    }
}

void COOPFunction::UpdateHistoryCommand(double _historycommand)
{
    for(int i = 0; i < 5; i++)
    {
        HistoryCommand(i) = HistoryCommand(i+1);
    }
    HistoryCommand(5) = _historycommand;
}

void COOPFunction::SettingMatrix()
{
    History_mean = HistoryCommand.mean();
    Eigen::VectorXd _History_std = HistoryCommand;
    for (int i = 0; i < HistoryCommand.rows(); i++)
    {
           _History_std(i)=_History_std(i)-History_mean;   
    }
    History_std=sqrt((_History_std.transpose()*_History_std)[0]/HistoryCommand.rows());
    // std::cout<<"The std is :"<<History_std<<std::endl;
    // 收益
    //           A11 增大         A12 减小
    // A21 靠近
    // A22 远离
    // 减小用户介入系数 与方差呈正相关
    double v_jianxiao_l = 1./(1+exp(-60*(History_std-0.1)));
    //  增大用户介入系数 与方差呈负相关
    double v_zengda_l = 1./(1+exp(60*(History_std-0.1)));
    // 靠近原点 与junzhi成负相关
    double v_kaojin_l = 1./(1+exp(60*(abs(History_mean)-0.1)));
    // 远离原点 与junzhi成正相关
    double v_yuanli_l = 1./(1+exp(-60*(abs(History_mean)-0.1)));
    // 计算收益
    double v_A11_A21 = v_zengda_l + v_kaojin_l;
    double v_A12_A21 = v_jianxiao_l + v_kaojin_l;
    double v_A11_A22 = v_zengda_l + v_yuanli_l;
    double v_A12_A22 = v_jianxiao_l + v_yuanli_l;

    Eigen::MatrixXd::Index maxRow, maxCol;
	// MatrixXd::Index minRow, minCol;
	Eigen::MatrixXd mMat(2,2);
	mMat << v_A11_A21,v_A12_A21,v_A11_A22,v_A12_A22;

	// double min = mMat.minCoeff(&minRow,&minCol);
	double max = mMat.maxCoeff(&maxRow,&maxCol);

	// std::cout << "Max = \n" << max << std::endl;
	// std::cout << "Min = \n" << min << std::endl;
	// std::cout << "minRow = " << minRow << "minCol = " << minCol << std::endl;
	// std::cout << "maxRow = " << maxRow << "maxCol = " << maxCol << std::endl;
    if(maxRow == 1)
    {
        Repulsor_point = Repulsor_point - 0.02;
    }
    else
    {
        Repulsor_point = Repulsor_point + 0.02;
    }
    if(Repulsor_point > 0.7)
    {
        Repulsor_point = 0.7;
    }
    if(Repulsor_point < 0.3)
    {
        Repulsor_point = 0.3;
    }
    if(maxCol == 1)
    {
        k_h = k_h + 0.02;
    }
    else
    {
        k_h = k_h - 0.02;
    }
    if(k_h > 0.95)
    {
        k_h = 0.95;
    }
    if(k_h < 0.6)
    {
        k_h = 0.6;
    }
}
void COOPFunction::Intialize_parameter(double _thhold_1,double _thhold_2)
{
    thhold_1 = _thhold_1;
    thhold_2 = _thhold_2;
}
void COOPFunction::VirtualForceRealUpdate(double x)
{
    Virtual_Force_Real = VirtualForcePredictUpdate(Virtual_Force_Real);
    Virtual_Force_Real += x;
}
}